/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-07-15     MBW       the first version
 */
#ifndef CONTROL_PID2_H_
#define CONTROL_PID2_H_
#include "sys.h"
//pid参数
typedef struct
{
    //自稳控制三轴 角度 环PID参数
    F_XYZ angel_kp , angel_ki , angel_kd ;
    //自稳控制三轴 角速度 环PID参数
    F_XYZ rate_kp , rate_ki , rate_kd ;
    //三轴 位置 环PID参数
    F_XYZ dis_kp , dis_ki , dis_kd ;
    //三轴 速度 环PID参数
    F_XYZ vel_kp , vel_ki , vel_kd ;
    //前馈，补偿控制参数
    float FC_P , FC_I , FC_D;
    //防漂移补偿角度
    float OffsetPitch , OffsetRoll ;
    //伺服电机PID
    float x_p , x_i , x_d , y_p , y_i , y_d;
    //光流补偿
    float fxp,fxi,fxd,fyp,fyi,fyd;
    //
} PID_param;
extern PID_param PID_PAR;
float cascade_pid_pitch_test(float eur,float mpu_eur,float mpu_gyro
                            ,float akp,float aki,float akd
                            ,float rkp,float rki,float rkd);
float cascade_pid_roll_test(float eur,float mpu_eur,float mpu_gyro
                            ,float akp,float aki,float akd
                            ,float rkp,float rki,float rkd);
float cascade_pid_yaw_test(float eur,float mpu_eur,float mpu_gyro
                            ,float akp,float aki,float akd
                            ,float rkp,float rki,float rkd);
float vel_pid_y_test(float set_vel ,float nav_vel
                    ,float vel_k,float vel_i,float vel_d);
float vel_pid_x_test(float set_vel ,float nav_vel
                    ,float vel_k,float vel_i,float vel_d);
float vel_pid_z_test(float set_vel ,float nav_vel
                            ,float vel_k,float vel_i,float vel_d);
float cas_height_pid_test(float set_h ,float nav_h,float nav_vz,float limit_vel_up,float limit_vel_down
                         ,float hp , float hi ,float hd
                         ,float vzp ,float vzi,float vzd );
float k210_sevrox_pid(float pos_set ,float pos_get
                    ,float pos_k,float pos_i,float pos_d,u8 sta);
float k210_sevroy_pid(float pos_set ,float pos_get
                    ,float pos_k,float pos_i,float pos_d,u8 sta);
float cas_velx_pid_test(float target ,float get_data1,float get_data2
                        ,float limit_vel_up,float limit_vel_down
                        ,float kp1 , float ki1 ,float kd1
                        ,float kp2 , float ki2, float kd2 );
float cas_vely_pid_test(float target ,float get_data1,float get_data2
                        ,float limit_vel_up,float limit_vel_down
                        ,float kp1 , float ki1 ,float kd1
                        ,float kp2 , float ki2, float kd2 );
float pid_testx(float target , float get_data , float kp , float ki , float kd);
float pid_testy(float target , float get_data , float kp , float ki , float kd);
float pid_dis_z(float target , float get_data , float kp , float ki , float kd);
float pid_vel_z(float target , float get_data , float kp , float ki , float kd);
#endif /* CONTROL_PID_H_ */
